#include "physics.h"

Physics::Physics() {
}

double Physics::collide(double p0[], double p1[], double r) {
	
	double dx[2];
	double dv[2];
	add1(dx, 0, 1.0, p1, 0, -1.0, p0, 0);		//double dx[] = add1(p1, 0, -1.0, p0, 0);
	add1(dv, 0, 1.0, p1, 2, -1.0, p0, 2);		//double dv[] = add1(p1, 2, -1.0, p0, 2);
	double dx2 = len2(dx, 0);
	double dv2 = len2(dv, 0);
	double xv = ip(dx, 0, dv, 0);
	double disc2 = xv * xv - dv2 * (dx2 - 4.0 * sqr(r));
	if(disc2 < 0.0 || dv2 == 0.0 || xv >= 0.0) {
		return numeric_limits<double>::infinity();
	}
	else {
		return max(-xv - sqrt(disc2), 0.0) / dv2;
	}
}

void Physics::rebound(double p0[], double p1[])	{
	
	double dx[2];
	double dv[2];
	add1(dx, 0, 1.0, p1, 0, -1.0, p0, 0);		//double dx[] = add1(p1, 0, -1.0, p0, 0);
	add1(dv, 0, 1.0, p1, 2, -1.0, p0, 2);		//double dv[] = add1(p1, 2, -1.0, p0, 2);
	double dx2 = len2(dx, 0);
	double xv = ip(dx, 0, dv, 0);
	double c = -xv / dx2;
	if(c > 0.0) {
		add1(p1, 2, p1, 2, c, dx, 0);
		add1(p0, 2, p0, 2, -c, dx, 0);
	}
}

void Physics::add1(double Z[], int iz, double ax, double X[], int ix, double ay, double Y[], int iy) {
	Z[iz] = ax * X[ix] + ay * Y[iy];
	Z[iz + 1] = ax * X[ix + 1] + ay * Y[iy + 1];
}

void Physics::add1(double Z[], int iz, double X[], int ix, double Y[], int iy) {
	return add1(Z, iz, 1.0, X, ix, 1.0, Y, iy);
}

void Physics::add1(double Z[], int iz, double X[], int ix, double ay, double Y[], int iy){
	return add1(Z, iz, 1.0, X, ix, ay, Y, iy);
}

double Physics::ip(double X[], int ix, double Y[], int iy) {
	return X[ix] * Y[iy] + X[ix + 1] * Y[iy + 1];
}

double Physics::len2(double X[], int ix) {
	return sqr(X[ix]) + sqr(X[ix + 1]);
}
 
double Physics::len(double X[], int ix) {
	return sqrt(len2(X, ix));
}
 
double Physics::len2(double X[], int ix, double Y[], int iy) {
	return sqr(X[ix] - Y[iy]) + sqr(X[ix + 1] - Y[iy + 1]);
}
 
double Physics::len(double X[], int ix, double Y[], int iy) {
	return sqrt(len2(X, ix, Y, iy));
}

double Physics::sqr(double x) {
	return x * x;
}